/*************
author:: libo
license:: MIT
*****************/

#include<Matrix/LB_Matrix.h>

#define Matrix4x4 LB_Matrix

//
// 分块的Householder变换进行qr分解，不论对稀疏矩阵还是稠矩阵都更好
// 要在复数域进行qr分解
// 
//
void lb_matrix_free(LB_Matrix *m)
{
	if(m==NULL)
	{return;}
	if(m->data!=0)
	{
		free(m->data);
	}
	free(m);
}
LB_MATRIX_FUNC(double)
LB_MATRIX_FUNC(float)
LB_Matrix *Projection(float FOV,float aspect,float zNear,float zFar)
{
    LB_Matrix *p=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_float(p,4,4);
	p->zero(p);
    float zRange=zNear-zFar;
    float tanHalfFOV=tan((FOV/2.0f));
	float* data=(float*)(p->data);
    data[0*4+0]=1.0/(tanHalfFOV*aspect);
    data[1*4+1]=1.0/tanHalfFOV;
    data[2*4+2]=-(zNear+zFar)/zRange;

    // data[2*4+3]=1.0;
    // data[3*4+2]= 2.0f*zFar*zNear/zRange ; 

    data[2*4+3]=2.0f*zFar*zNear/zRange;
    data[3*4+2]=-1.0f;

	return p;
}

//相机内参转透视矩阵
//返回FOV,ASPECT, 近裁面的距离
double* camera2Proj(double*ssd,double f)
{
    double* re=(double*)malloc(sizeof(double)*3);
    re[0]=atan((ssd[1]/2.0)/f );
    re[1]=ssd[0]/ssd[1];
    re[2]=f;

    return re;
}

static double gram_inner_product(double* a,LB_Matrix* G,double *b)
{
    double re=0;
    double*G_data=(double*)(G->data);
    for(int i=0;i<G->rows;i++)
    {
        for(int j=0;j<G->cols;j++)
        {
            re+=(G_data[i*G->cols+j]*a[i]*b[j]);
        }
    }

    return re;
}

static void zuobiaohuanyun(double**as,int row,int col, double* lamb,double*re )
{
    memset(re,0,sizeof(double)*col);
    for(int i=0;i<row;i++)
    {
        for(int j=0;j<col;j++)
        {
            re[j]+=as[i][j]*lamb[i];
        }
    }

}


double*  compute_high_dim_circumscribed_sphere(double**M,int row,int col)
{
    if(row<=1){return NULL;}
    double** as=(double**)malloc(sizeof(double*)*(row-1));
    for(int i=0;i<row-1;i++)
    {
        as[i]=(double*)malloc(sizeof(double)*col);
        for(int j=0;j<col;j++)
        {
            as[i][j]=M[i+1][j]-M[0][j];
        }
    }

    double * re=(double*)malloc(sizeof(double)*col);
    memset(re,0,sizeof(double)*col);

    LB_Matrix* G=(LB_Matrix*)malloc(sizeof(LB_Matrix));

    lb_matrix_init_double(G,row-1,row-1);
    
    double*G_data=(double*)(G->data);

    for(int i=0;i<row-1;i++)
    {
        for(int j=0;j<row-1;j++)
        {
            G_data[i*(row-1)+j]=inner_product(as[i],as[j],col);
        }
    }

    LB_Matrix* G_inverse=G->inverse(G);
    double*Gi_data=(double*)(G_inverse->data);
    double * rei=(double*)malloc(sizeof(double)*col);
    memset(rei,0,sizeof(double)*col);
    for(int i=0;i<row-1;i++)
    {
        memset(rei,0,sizeof(double)*col);
        zuobiaohuanyun(as,row-1,col,&(Gi_data[i*(row-1)]),rei );

        double norm= Gi_data[i*(row-1)+i];
      
        for(int j=0;j<col;j++)
        {
            re[j]+=((rei[j]*0.5)/norm);
        }
    }
    for(int i=0;i<col;i++)
    {
        re[i]+=M[0][i];
    }



    free(rei);
    lb_matrix_free(G);


    for(int i=0;i<row-1;i++)
    {
        free(as[i]);
    }
    free(as);
    return re;
}

double * compute_antisymmetric_tensor(double**M,int rows,int cols,double* re,int* re_len)
{
    if(rows>cols){return NULL;}

    int* c=NULL;
    c=compute_combination(rows,cols,c,re_len);
    
    if(re==NULL)
    {
        // *re_len=1;
        // for(int i=0;i<rows;i++)
        // {
        //     (*re_len)*=(cols-i);     
        // }
        // (*re_len)=(*re_len)/factorial(cols);

    //printf("%d\n",len);
        re=(double*)malloc(sizeof(double)*(*re_len));
    }
    LB_Matrix mat;
    lb_matrix_init_double(&mat,rows,rows);
    double * data=(double*)(mat.data);

    int in=-(rows-1)*rows/2;
    for(int i=0;i<*re_len;i++)
    {
        int temp=in;
        for(int j=0;j<rows;j++)
        {
            int jj=c[i*rows+j];
            for(int k=0;k<rows;k++)
            {
                data[k*rows+j]=M[k][jj];
            }
            temp+=jj;
        }
        double* det=(double*)(mat.det(&mat));
        re[i]=*det;
        free(det);
        if(temp%2==1)
        {
            re[i]=-re[i];
        }
    }

    free(data);

    return re;
}

// #define N 3  // 矩阵的维度
// LU分解（带部分选主元）
void LUDecomposition(double **A, double **L, double **U, int *P,int n) 
{
    // 初始化L为单位矩阵，U为A的副本，P为置换矩阵
    for (int i = 0; i < n; i++) {
        P[i] = i;  // 初始化置换矩阵
        for (int j = 0; j < n; j++) {
            U[i][j] = A[i][j];
            L[i][j] = (i == j) ? 1.0 : 0.0;
        }
    }

    for (int k = 0; k < n; k++) {
        // 选主元：找到第k列中绝对值最大的行
        int maxRow = k;
        double maxVal = fabs(U[k][k]);
        for (int i = k + 1; i < n; i++) {
            if (fabs(U[i][k]) > maxVal) {
                maxVal = fabs(U[i][k]);
                maxRow = i;
            }
        }

        // 交换行
        if (maxRow != k) {
            // 交换U的行
            for (int j = 0; j < n; j++) {
                double temp = U[k][j];
                U[k][j] = U[maxRow][j];
                U[maxRow][j] = temp;
            }

            // 交换L的行（只交换已经计算的部分）
            for (int j = 0; j < k; j++) {
                double temp = L[k][j];
                L[k][j] = L[maxRow][j];
                L[maxRow][j] = temp;
            }

            // 更新置换矩阵
            int temp = P[k];
            P[k] = P[maxRow];
            P[maxRow] = temp;
        }

        // LU分解
        for (int i = k + 1; i < n; i++) {
            L[i][k] = U[i][k] / U[k][k];  // 计算L的第i行第k列元素
            for (int j = k; j < n; j++) {
                U[i][j] -= L[i][k] * U[k][j];  // 更新U的第i行
            }
        }
    }
}

#undef Matrix4x4
